#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <sys/time.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "common/getopt.h"
#include "common/serial.h"
#include "common/ioutils.h"
#include "common/timestamp.h"

#include "common/getopt.h"
#include "lcmtypes/pimu_t.h"
#include "lcm/lcm.h"

#define BUFLEN 1024

void callback(void *_context, int64_t ts, const char *buf);

typedef struct pimud pimud_t;
struct pimud
{
    getopt_t *gopt;
    lcm_t *lcm;

    int fd;
    int quit;

    uint8_t buf[BUFLEN];
    int buf_pos;
    int buf_len;
};


static int my_read(pimud_t *pimud)
{
    if (pimud->buf_pos == pimud->buf_len) {
        pimud->buf_pos = 0;
        int res = read(pimud->fd, pimud->buf, BUFLEN);
        assert(res > 0);
        pimud->buf_len = res;
    }

    return pimud->buf[pimud->buf_pos++];
}

static int32_t read_s16(uint8_t *p)
{
    int16_t v = (p[0]<<8) + p[1];
    return v;
}

static uint32_t read_u32(uint8_t *p)
{
    return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + (p[3]);
}

static uint64_t read_u64(uint8_t *p)
{
    uint64_t d0 = read_u32(p);
    uint64_t d1 = read_u32(&p[4]);
    return (d0<<32) + d1;
}

static uint32_t checksum_init()
{
    return 0x12345678;
}

static uint32_t checksum_update(int32_t chk, uint32_t c)
{
    chk = chk + (c & 0xff);
    chk = (chk << 1) ^ (chk >> 23); // java version relies on signed chk here.
    return chk;
}

static uint32_t checksum_finish(uint32_t chk)
{
    return (chk & 0xff) ^ ((chk >> 8) & 0xff) ^ ((chk >> 16) & 0xff);
}

static void cleanup(int s, void *arg)
{
    pimud_t *pimud = (pimud_t *)arg;
    pimud->quit = 1;
    printf("done\n");
}

static void sighandler(int s)
{
	switch(s) {
        case SIGINT:
        case SIGQUIT:
        case SIGTERM:
            printf("Exiting...");
            exit(s);
        default:
            break;
	}
}

void pimud_usbmod_workaround(char *device)
{
    FILE *fp;
    char line[256];

    fp = popen("sudo reset_usb.sh", "r");
    if (fp == NULL) {
        printf("Command (usbmod_workaround) failed\n");
        exit(1);
    }

    while (fgets(line, sizeof(line)-1, fp) != NULL) {
        printf("%s", line);
    }
    pclose(fp);
}

int main(int argc, char *argv[])
{
    pimud_t *pimud = (pimud_t*) calloc(1, sizeof(pimud_t));

    ///// Signal handling /////
    on_exit(cleanup, pimud);
    signal(SIGINT, sighandler);
    signal(SIGQUIT, sighandler);
    signal(SIGTERM, sighandler);

    pimud->gopt = getopt_create();
    getopt_add_string(pimud->gopt, 'd', "device", "/dev/ttyUSB0", "PIMU serial device");
    getopt_add_string(pimud->gopt, 'c', "channel", "PIMU", "LCM publish channel");
    getopt_add_int(pimud->gopt,    'b', "baud", "230400", "Serial baud rate");
    getopt_add_int(pimud->gopt,    '\0', "decimate", "1", "Transmit only every Nth message");
    getopt_add_bool(pimud->gopt,   'u', "usbmod", 0, "Disable usbmod workaround (-u turn off workaround)");
    getopt_add_bool(pimud->gopt,   'v', "verbose", 0, "Verbose");

    if (!getopt_parse(pimud->gopt, argc, argv, 1)) {
        getopt_do_usage(pimud->gopt);
        return 0;
    }

    pimud->lcm = lcm_create(NULL);

    if (!getopt_get_bool(pimud->gopt, "usbmod")) {
        printf("Running reset_usb.sh...\n");
        pimud_usbmod_workaround(getopt_get_string(pimud->gopt, "device"));
        usleep(500000);
        printf("done\n");
    }
    pimud->fd = serial_open(getopt_get_string(pimud->gopt, "device"), getopt_get_int(pimud->gopt, "baud"), 1);
    if (pimud->fd < 0) {
        perror(getopt_get_string(pimud->gopt, "device"));
        exit(1);
    }
    char *channel = getopt_get_string(pimud->gopt, "channel");
    printf("Publishing on channel: %s\n", channel);
    printf("Using device %s\n", getopt_get_string(pimud->gopt, "device"));

    int rx_count = 0, tx_count = 0;
    int decimate = getopt_get_int(pimud->gopt, "decimate");

    int64_t last_report_utime = 0;
    int64_t utime0 = timestamp_now();

    while (!pimud->quit) {
        // synchronize
        while (!pimud->quit) {
            if (my_read(pimud) != 0xed)
                continue;
            if (my_read(pimud) != 0x87)
                continue;
            if (my_read(pimud) != 0x71)
                continue;
            if (my_read(pimud) != 0xe2)
                continue;
            break;
        }

        uint32_t chk = checksum_init();

        uint8_t len0 = my_read(pimud);
        chk = checksum_update(chk, len0);

        uint8_t len1 = my_read(pimud);
        chk = checksum_update(chk, len1);

        uint32_t len = (len0<<8) + len1;
        if (len > 1024)
            continue;

        uint8_t buf[len];

        for (int i = 0; i < len; i++) {
            buf[i] = my_read(pimud);
            chk = checksum_update(chk, buf[i]);
        }

        chk = checksum_finish(chk);

        uint8_t readChk = my_read(pimud);

        if (readChk != chk) {
            printf("bad checksum %02x %02x\n", readChk, chk);
            continue;
        }

        pimu_t pimu;
        pimu.utime = timestamp_now();

        int pos = 2;
        pimu.utime_pimu = read_u64(&buf[pos]);  pos+=8;

        for (int i = 0; i < 8; i++) {
            pimu.integrator[i] = read_u32(&buf[pos]); pos+=4;
        }

        for (int i = 0; i < 3; i++) {
            pimu.accel[i] = read_s16(&buf[pos]); pos+=2;
        }

        for (int i = 0; i < 3; i++) {
            pimu.mag[i] = read_s16(&buf[pos]); pos+=2;
        }

        for (int i = 0; i < 2; i++) {
            pimu.alttemp[i] = read_u32(&buf[pos]); pos+=4;
        }

        rx_count++;

        if ((rx_count % decimate) == 0) {
            pimu_t_publish(pimud->lcm, channel, &pimu);
            tx_count++;
        }

        if (pimu.utime - last_report_utime > 1E6) {
            double dt = (pimu.utime - last_report_utime) / 1000000.0;

            printf("PIMUD %15.3f : RX %6.1f Hz, TX %6.1f Hz\n", (pimu.utime - utime0) / 1000.0, rx_count / dt, tx_count / dt);

            last_report_utime = pimu.utime;
            rx_count = 0;
            tx_count = 0;
        }
    }

    return 0;
}
